import math
from dubins import Dubins_path
import matplotlib.pyplot as plt
from math import  sin, cos
from FWDM import FWDM
def showpath(paths_x=[],paths_y=[],ponits = []):
    global M_type
    plt.rcParams['xtick.direction'] = 'in'  # 将x轴的刻度线方向设置向内
    plt.rcParams['ytick.direction'] = 'in'
    plt.grid(False)
    plt.figure(figsize=(6.3, 3))
    plt.rcParams['font.family']='Times New Roman ,SimSun '
    plt.xlabel('Latitude ',fontsize=8 )
    plt.ylabel('Longitude',fontsize=8 )
    plt.xticks(size=8)
    plt.yticks(size=8)
    for tx in range(len(paths_x)):
        paths_x[tx] /= 100
    for ty in range(len(paths_y)):
        paths_y[ty] /= 100
    for ts in ponits:
        ts[0] /= 100
        ts[1] /= 100
    if len(ponits) != 0:
        arrow_length = 0.2
        for i in ponits:
            Pi = [i[0],i[1]]
            plt.arrow(Pi[0],
                      Pi[1],
                      arrow_length * cos(i[2]),
                      arrow_length * sin(i[2]),
                      width=0.01,
                      length_includes_head=True,
                      head_width=0.05,
                      head_length=0.01,
                      fc='g',
                      ec='g')
            break
    for i in range(len(paths_x) - 1):
        start = [paths_x[i], paths_x[i + 1]]
        end = [paths_y[i], paths_y[i + 1]]
        plt.plot(start, end, color = (100/255, 149/255, 237/255), linewidth=1)
    if len(ponits) != 0:
        for i in range(1,len(ponits) - 1,2):
            s = [ponits[i][0],ponits[i + 1][0]]
            e = [ponits[i][1],ponits[i + 1][1]]
            plt.scatter(s[0],e[0],s = 1) # 起止点,切点
            plt.scatter(s[1], e[1],s = 1)  # 起止点,切点
            plt.plot(s, e, c = 'red', linewidth = 1.2)
    plt.axis('equal')
    plt.show()
def dubins_path(point_s = [],point_e = []):
    if len(point_s) < 3 or len(point_e) < 3:
        return
    if point_s[2] == point_e[2]:
        return [point_s[0],point_e[0]],[point_s[1],point_e[1]],math.sqrt((point_s[0]-point_e[0])**2 + (point_s[1] - point_e[1]) ** 2)
    Pi = [point_s[0], point_s[1]]
    Pf = [point_e[0], point_e[1]]
    alpha = point_s[2]
    beta = point_e[2]
    trun_types = ["RSR", "LSL", "RSL", "LSR"]
    res_path_x = []
    res_path_y = []
    min_dist = -1
    for i in trun_types:
        tmp_res_path_x, tmp_res_path_y, dis_lst = Dubins_path(Pi=Pi, Pf=Pf, alpha=alpha, beta=beta, ratio=20, tp=i,
                                                              show_path=False)
        if dis_lst == -1:
            continue
        if dis_lst == 0:
            continue
        if min_dist == -1 or dis_lst < min_dist:
            min_dist = dis_lst
            res_path_x = tmp_res_path_x.copy()
            res_path_y = tmp_res_path_y.copy()
    return res_path_x, res_path_y, min_dist
    # 显示图像
if __name__ == "__main__":
    fwdm = FWDM()
    RES_X,RES_Y = fwdm.Run()
    #输出结果
    path_points = []
    path_points.append(fwdm.mis_points)
    for i in range(len(RES_X)):
        #first
        ind = RES_X[i]
        if ind < fwdm.ub:
            if RES_Y[i] == 0:
                point_s = fwdm.TaskLine[ind][0]
                point_e = fwdm.TaskLine[ind][1]
                point_s.append(math.atan2((point_e[1] - point_s[1]), (point_e[0] - point_s[0])) )#(180 / math.pi)
                point_e.append(math.atan2((point_e[1] - point_s[1]), (point_e[0] - point_s[0])) )
                path_points.append(point_s)
                path_points.append(point_e)
            else:
                point_s = fwdm.TaskLine[ind][1]
                point_e = fwdm.TaskLine[ind][0]
                point_s.append(math.atan2((point_e[1] - point_s[1]), (point_e[0] - point_s[0]))  )
                point_e.append(math.atan2((point_e[1] - point_s[1]), (point_e[0] - point_s[0])) )
                path_points.append(point_s)
                path_points.append(point_e)
        else:
            pass
    px = []
    py = []
    pathlen = 0
    for i in range(len(path_points)):
        if i == len(path_points) - 1:#到机场
            ponit_s = path_points[i]
            point_e = path_points[0]
        else:
            ponit_s = path_points[i]
            point_e = path_points[i + 1]
        tmppx, tmppy, tmplen = dubins_path(ponit_s, point_e)
        pathlen += tmplen
        for i in range(len(tmppx)):
            px.append(tmppx[i])
            py.append(tmppy[i])
    showpath(px, py, path_points)

